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Abstract — Curves  and  natural  frames  can  be  used  for  de¬ 
scribing  and  controlling  motion  in  both  biological  and  en¬ 
gineering  contexts  (e.g.,  pursuit  and  formation  control).  The 
geometry  of  curves  and  frames  leads  naturally  to  a  Lie 
group  formulation  where  coordinated  motion  is  represented 
by  interacting  particles  on  Lie  groups  -  specifically,  SE( 2)  or 
SE(3).  Here  we  consider  a  particular  type  of  optimal  control 
problem  in  which  the  interactions  between  particles  arise  from 
a  cost  function  dependent  on  each  particle’s  steering,  and  which 
penalizes  steering  differences  between  the  particles  (expressed 
via  the  graph  Laplacian).  With  this  choice  of  cost  function, 
we  are  able  to  perform  Lie-Poisson  reduction.  Furthermore, 
we  are  able  to  derive  a  closed-form  expression  (using  Jacobi 
elliptic  functions)  for  certain  special  solutions  of  the  coupled 
multi-particle  problem  on  SE( 2). 

I.  Introduction 

It  is  convenient  to  decompose  the  problem  of  multi-vehicle 
coordinated  control  into  two  tasks:  trajectory  generation 
(based  on  pursuit,  formation  flight,  or  some  other  mission- 
guided  strategy),  and  trajectory  tracking  (the  role  assigned 
to  the  autopilot).  Similarly,  in  analyzing  data  and  testing  hy¬ 
potheses  about  biological  motion  it  is  convenient  to  separate 
the  task  of  trajectory  reconstruction  (based  on  measured  data) 
from  the  problem  of  trajectory  generation  by  the  animal  (as 
it  implements  a  particular  strategy)  [5],  [16].  Inspired  by 
considerations  of  high-speed  vehicles  and  UAVs,  the  task  of 
trajectory  generation  can  be  formulated  in  terms  of  constant- 
speed  particle  motion,  or  equivalently,  in  terms  of  curves  and 
moving  frames.  By  packaging  the  curve  and  frame  equations 
as  left-invariant  systems  on  SE( 2)  (for  planar  curves)  or 
SE( 3)  (for  curves  in  three-dimensional  space),  we  are  led 
to  interaction  laws  for  particles  in  Lie  groups  as  a  basis  for 
designing  coordinated  vehicle  trajectories  for  tasks  such  as 
pursuit,  formation  flight,  and  boundary  tracking  [8],  [9],  [17], 

[11],  [18].  These  tasks  have  been  formulated  using  relative 
shape ,  i.e.,  the  system  has  a  group  symmetry,  which  the  Lie 
group  formulation  allows  us  to  exploit. 

In  this  paper,  we  retain  the  Lie  group  formulation  of  curves 
and  moving  frames  for  describing  trajectories  of  multiple 
coordinated  vehicles,  but  we  consider  a  different  type  of 
strategy:  collective  optimization  of  trajectories  subject  to 
fixed  endpoint  conditions.  From  a  practical  standpoint,  this 
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type  of  strategy  could  be  appropriate  if  it  is  desired  to 
coordinate  the  arrival  of  particular  vehicles  at  particular 
locations  at  scheduled  points  in  time.  Here  we  consider 
minimizing  the  cumulative  “steering  energy”  for  the  in¬ 
dividual  particles,  along  with  “steering  difference  energy” 
for  multiple  particles.  For  a  single  particle,  minimizing  the 
steering  energy  is  a  reasonable  objective  for  certain  types  of 
vehicles  (particularly  high-speed  vehicles),  because  turning 
requires  extra  fuel  consumption,  reduces  dynamic  stability 
(e.g.,  leading  to  roll-over  in  a  high-speed  ground  vehicle), 
and  places  more  demands  on  an  autopilot  than  traveling  in 
a  straight  line.  For  multiple  vehicles  in  proximity  to  each 
other,  and  moving  at  roughly  the  same  speed  (and  in  roughly 
the  same  direction),  minimizing  the  differences  among  their 
steering  controls  is  a  technique  for  reducing  the  likelihood 
of  collision.  The  advantage  of  penalizing  steering  control 
differences,  rather  than  inter-vehicle  distances,  is  that  Lie- 
Poisson  reduction  can  be  applied  to  simplify  the  description 
of  the  interacting  particle  dynamics,  which  is  the  main  theme 
of  this  paper. 

To  understand  the  advantage  of  Lie-Poisson  reduction,  it 
is  helpful  to  consider  the  various  types  of  reduction  used 
in  mechanics  [7],  [15].  Poisson  reduction  can  be  applied  to 
various  mechanics  problems  involving  coupled  rigid  bodies 
to  reduce  the  dimension  of  the  space  in  which  solutions 
need  to  be  computed.  For  example,  the  overall  rigid  motion 
symmetry  in  a  coupled  two-body  problem  considered  in  [6], 

[12]  allows  Poisson  reduction  from  the  state  space 

T*SO( 3)  x  T*SO( 3)  £*  50(3)  x  50(3)  x  so*  (3)  x  so*  (3) 

to  the  reduced  space  50(3)  x  so* (3)  x  so*  (3).  However,  in 
mechanical  problems  it  is  unusual  to  have  further  reduction 
associated  with  Lie-Poisson  (in  this  example,  to  so*  (3)  x 
so*  (3)).  There  are  examples  of  such  Lie  Poisson  reduction 
in  physics,  e.g.,  nonlinear  optical  polarization  dynamics  [2], 

[13] ,  but  here  we  focus  on  optimal  control  problems  which 
admit  Lie  Poisson  reduction. 

This  paper  is  organized  as  follows.  We  first  formulate 
a  general  optimal  control  problem  for  multiple  particles 
in  an  arbitrary  matrix  Lie  group.  The  particles  are  cost- 
coupled  through  a  connected,  undirected  graph.  The  Max¬ 
imum  Principle  is  applied,  and  the  first-order  necessary 
conditions  for  regular  extremals  are  derived.  Furthermore, 
the  Lie-Poisson  reduced  equations  are  computed.  Next,  we 
specialize  to  SE( 2),  and  express  the  reduced  equations  in 
a  more  concrete  form.  For  a  single  particle  in  SE( 2),  the 
reduced  equations  can  be  solved  in  closed  form  using  Jacobi 
elliptic  functions  -  indeed,  this  is  a  simple  example  from 
elastica  theory  [7].  What  we  show  is  that  there  are  special 
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solutions  for  the  multi-particle  reduced  dynamics  which  can 
also  be  written  in  closed  form  using  Jacobi  elliptic  functions. 
The  conclusion  is  that  for  certain  special  fixed-endpoint 
conditions,  the  optimal  multi-particle  steering  law  (where  the 
cost  function  includes  both  “steering  energy”  and  “steering 
difference  energy”  terms)  consists  of  steering  controls  for  the 
individual  particles  which  are  all  proportional  to  one  another. 
Dedication:  It  is  a  pleasure  to  dedicate  this  work  to  Tudor 
Stefan  Ratiu  on  the  occasion  of  his  sixtieth  birthday. 

II.  Optimal  control  problem 


A.  Problem  formulation 

Consider  a  connected,  undirected  graph  with  vertices 
Vi,  V2,  ■■■,  Vn  G  V,  without  self  loops,  and  denote  the  degree 
of  vertex  vl  by  d(vf),  i  =  1  The  adjacency  matrix 

A  is  then  defined  by  a,,  =  1  if  vertices  Vi  and  Vj  are 
connected,  and  alt  =  0  otherwise,  i,j  =  1,  ...,7V.  We  define 
the  degree  matrix  D  =  diag(d(t>i), ...,  d(vN )),  and  the  graph 
Laplacian  by  B  =  D  —  A,  where  we  note  that  A,  D,  and  B 
are  symmetric  matrices  [4]. 

We  seek  to  minimize 

C=  f  L(G(t),...,fN(t))dt,  (1) 

Jo 

subject  to  the  controlled  dynamics  g k  =  gkG>  9k  G  G , 
a  matrix  Lie  group  (having  real-valued  entries),  with  fixed 
endpoints  gfc(0)  =  gko,  9k(T)  =  gkr,  k  =  l,...,N,  where 
the  Lagrangian 

1  /  N  N  N 

l(€u—,€n)  =  9  l£fcl2  +  xy^y^afcjl£fc 


,k=l 


k=  1  j=  1 


(2) 

with  %  >  0  a  constant.  We  use  the  trace  norm  |£|2  = 
tr  (£T£)  j  and  inner  product  (£,77)  =  tr(£T7y),  £,  77  G  g, 
where  g  is  the  Lie  algebra  associated  with  G.  We  have 

N  N  N  N 

^2 ^2 akj\&  - £ji2  =  2^2 ^2 hkJ  (&>&)  > 

k—l j—1  k—l j—1 

where  we  have  used  the  symmetry  of  A,  and  thus  we  can 
rewrite  (2)  as 

n  n  1  n  \ 

L(€u —>€n)  =  2 ^2 i^i2 + x^2 ( ^ 

k—l  k—l  \  j—1  / 


Remark:  Note  that  the  expression  (G-  —  G)  is  usecl  in  the 
Lagrangian  (2),  rather  than  (£k  -  Adfl-i^-),  g  =  g~1gk, 
which  would  be  more  natural  from  a  mechanics  viewpoint. 
The  attitude  we  take  here  is  to  posit  a  Lagrangian,  solve 
the  corresponding  optimal  control  problem,  and  then  assess 
whether  the  resulting  analysis  (specifically,  the  form  of  the 
Lie-Poisson  reduced  dynamics)  provides  any  useful  insight 
regarding  control  of  particle  collectives. 

We  take  G  to  be  affine  in  the  control  vector  uk  for  each 
k,  i.e.,  we  take 

m 

G  =  Xg  +  ukjXj,  k  =  l,...,N,  (5) 

*=1 


where  uk  =  {ukl, ukm)  G  Mm,  {XUX2,  ...,Xn}  is  a 
basis  (assumed  to  be  orthonormal  with  respect  to  the  trace 
inner  product)  for  the  Lie  algebra  g,  m  <  n,  and  q  G  {(m  + 
l),...,n}.  Thus,  the  system  is  underactuated  and  has  drift. 
(Replacing  Xq  in  (5)  with  zero  yields  a  driftless  system.) 
With  the  substitution  (5)  we  can  write  L  =  L(ui,  ...,un). 


B.  Maximum  Principle 

Restricting  attention  to  regular  extremals  of  the  fixed 
endpoint  problem  posed  in  the  previous  subsection,  we  define 
the  pre-hamiltonian 

H(p,g,u)  =  {p,g£u)  ~  L(u)  (6) 

where  p  G  T*GN ,  the  cotangent  space  at  g,  and  GN  denotes 
the  direct  product  of  N  copies  of  the  Lie  group  G.  Here 

g  =  diag(g1,g2,...,gN),  G  =  diag(£i,£2,-~,6v),  (7) 

i.e.,  g  and  G  are  block-diagonal  matrices,  and  u  = 
(iti,  u2, Ujv)  is  the  control  vector  (of  length  mN).  Then 
(6)  can  be  written  as 


H{p,g,u)  =  H(pi,...,pN,gu—,gN,u1,...,uN) 

=  (y;  ( Pk,gkG -  L(m, ...,  uN),  (8) 

where  pk  G  T*kG  for  k  =  1, ....  A?.  The  Maximum  Principle 
then  states  that  if  ui,  ...,Un  minimizes  C,  gi, gw  denotes 
the  corresponding  trajectory  in  GN ,  and  the  only  extremals 
of  C  are  regular  extremals,  then 


H{p1,...,pN,g1,...,gN) 

=  sup  H(p1,...,pN,g1,...,gN,v1,...,vN),  (9) 
k=l,...,N 

for  a.e.  t  G  [0,T],  were  H  is  the  hamiltonian  [7],  [14],  The 
first-order  necessary  condition  for  (9)  is  dH/duki  =  0,  or 


d 

duki 


N 


^2{pj^g^j)  \  - l(ui,...,un) 
u'=t  / 

dG  \  dL 


=  \Pk,gk 


du 


ki 


du 


ki 


0,  GO) 


for  k  =  1, ...,  N  and  i  =  1, ...,  m,  where  (ui, ...,  ujy)  denote 
the  optimal  controls. 

Using  (5),  we  have  dG/duki  =  X,,  and  from  (4),  we 
compute 


dL 

duki 


N 

uki  +  2%y ^bjkUjj. 
i=i 


(11) 


Also, 


Pk->  9k 


dG 

duki 


(Pki  9kXf)  —  Pkii  (l2) 


where  the  translation  to  the  identity  of  pk  is  given  by  gk  G 
g*,  the  dual  space  of  the  Lie  algebra  of  G,  and 


pk  =  y^PkjX'l,  (13) 

i= 1 
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where  {X\,  X^}  is  the  dual  basis  to  {X1,X2, ...,  Xn}. 

Thus,  (10)  becomes 


JV 

Hki  =  uki  +  2x^bjkUji,  k  =  l,...,N,  i  =  1, ..., to.  (14) 
i= i 

Defining  /ife  =  [  g,kl  g k2  ■  ■  ■  fx km  ] T  ,  we  have 

JV 

Mfc  —  txk  T  2%  ^  '  bjk  U'j,  (15) 

i=ti 


or 


Mi 

Ml 

—  ((IjV  +  2\jB)  g  I  in) 

Mat 

Mat 

where  (g)  denotes  the  Kronecker  product.  For  convenience, 
we  define 


'f  —  ((Ijv  +  2x_B)  <g>  Im)  —  (Ijv  +  2\'i?)  (g  Im)  (17) 


assuming  that  the  inverse  exists.  All  of  the  eigenvalues  of 
B  are  real  and  nonnegative,  including  (at  least)  one  zero 
eigenvalue  [4].  Therefore,  T  is  guaranteed  to  exist  for  X  >  0. 
We  then  have 


Ml 

Mi 

=  T 

Mjv 

and  substituting  the  optimal  controls  back  into  the  hamilto- 
nian  gives 


JV 


where  we  recall  that  jlk  encodes  the  first  m  components  of 
/xk  for  each  k  =  1, ...,  TV. 

Defining  jx  =  [ fxi  g,2  ■■■  /i^r]  ,  so  that  fx  is  a 

vector  of  length  fi  =  Nn ,  the  dynamics  for  jx  are 


with 


fl  =  A(jx)Xh  =  A  (jx) 


dh/djx  i 
dh/djx  2 


ph/djin 


(22) 


A(m)  = 


Ml  k 

0 

0  ‘ 

"  p/c 

1  11 

p/c 

1  12 

rln 

n 

-y 

0 

M2fc 

0 

pfe 

1  21 

p/c 

1  22 

r 2n 

k= 1 

_  0 

0  Mivfc_ 

p/c 
_  nl 

p/c 

1  n2 

F 

nn  _ 

(23) 


where  T*:;  are  the  structure  constants  for  g,  and  all  of  the 
coupling  between  the  [xk,  k  =  1  in  the  reduced 

equations  are  due  to  Xh.  Due  to  the  block  diagonal  structure 
of  (23),  each  Casimir,  invariant  on  g*,  associated  with  G 
contributes  N  Casimirs  to  the  reduced  system  (22). 


III.  Specialization  to  G  =  SE( 2) 

By  specializing  to  G  =  SE( 2),  we  can  carry  the  cal¬ 
culation  further.  Consider  N  coupled  particles  in  SE(2): 
specifically,  equation  (5)  becomes 

£k  =  X2  +  ukX1,  k  =  1, ...,  N,  (24) 


H  =  yd  (Mfc,gfc)  -  L(uU:.,un) 

k= t 

JV  /  JV  N 

=  y  {vk,€k)-  J  2  y  1&12 + xy 

k=  1  V  k= 1  k=l 


/  N 

£k,  'yi  bkjt;j 

\  i 


(19) 


After  some  calculation,  we  obtain 
N  1 

H  =  yVkq+2  [  Al  •••  MJV  ]  V 
k=l 

where  our  assumption  that  B  is  symmetric  implies  that 
T  is  also  symmetric.  H,  being  independent  of  g,  permits 
reduction. 

C.  Lie-Poisson  reduction 


Mi 


Mat 


N’  (20) 


where  uk  is  a  scalar  steering  control  for  each  k,  and  the  basis 
we  use  for  the  Lie  algebra  se( 2)  is 


X,= 


V2 


'0  -1  o' 

i— 1 

o 

o 

o 

o 

o 

10  0 

II 

* 

0  0  0 

* 

00 

II 

0  0  1 

1 - 

o 

o 

o 

1 _ 

1 

o 

o 

o 

1 _ 

0  0  0 

which  is  normalized  so  that  |A'i| 
respect  to  the  trace  norm. 

From  (21)  we  have 


JV 


1 


h  —  y^  pk2  +  2  [  Mil 

fc= l 


=  \x2\  =  |X3|  =  1  with 


Mjv  i  If 


Mu 


Mjv  i 


•  (26) 


The  Lie-Poisson  reduced  dynamics  are  given  by  (22)  and 
(23),  where  now 


The  process  of  Lie-Poisson  reduction  takes  the  original 
system  on  ( T*G)N  and  reduces  it  to  a  system  on  (g*)^, 
with  the  reduced  variables  defined  as  )xk,  k  =  1, ...,  N.  The 
reduced  hamiltonian  has  already  been  computed  as  (20):  we 
can  ignore  the  constant  term  and  write 


A(m)  =  - 


72 


fii 

0 


0 

1^2 


with 


N  1 

h  =  y  Pkq  +  2  [  Mi  •  • 

Mat  ]  vf 

Mi 

,  (21) 

0 

Mfc3 

-~Pk2 

Qk  - 

— Mfc3 

0 

0 

k= 1 

Mat 

Mfc2 

0 

0 

0  fi 


JV 


(27) 


k  =  l,...,N.  (28) 
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Also, 

dh 

dp 


k  =  1, N,  with 


dh/dpi 

dh/dp2 

,  dh/dpk  = 

dh/dpk  1 

1 

dh/ dpN 

0 

dh/dpn 

JHl 

dh/dp2i 

=  T 

F21 

dh/dpNi 

Pn  1 

and  =  (Ijv  +  2 \B)~ 
A.  Motivation 


(29) 


(30) 


The  optimization  problem  we  pose  for  coupled  particles 
in  SE( 2)  can  be  viewed  as  an  optimal  control  problem  for 
framed  curves  in  R2,  where  the  objective  is  to  minimize 
the  curvature  (and  curvature  differences)  subject  to  fixed 
endpoint  conditions.  Framed  curves  in  R2  can  be  defined 
by  the  system 


r  =  x,  x  =  yu,  y  = -xu,  (31) 


where  r  £  R2  is  the  position  vector,  x  £  R2  is  the  unit 
tangent  vector  to  the  trajectory,  y  £  R2  is  the  unit  normal 
vector,  and  u  is  the  plane  curvature  (i.e.,  the  steering  control). 
Here,  we  assume  unit-speed  motion,  so  that  time  t  is  also 
the  arc  length  parameter.  (Note  that  u  in  (31)  differs  from 
Ufc  in  (24)  by  a  factor  of  1/ y/2,  due  to  our  normalization  of 
the  basis  for  se(2).)  An  alternative  way  to  express  (31)  is 


6  =  u,  fi  = 


cos  9 
sin  9  ’ 


(32) 


where  6  is  the  angle  associated  with  the  unit  tangent  vector 
to  the  trajectory. 


For  certain  values  of  h,  this  second-order  cubic  equation 
has  elliptic  function  solutions  [3].  To  obtain  these  solutions 
explicitly,  we  introduce  the  variables 

Hi{t)  =  <jy{v{t-rj)),  (37) 

where  o  >  0,  ry,  and  v  >  0  are  constant.  Then 

<jv2y"(y{t  -  r/j)  -  ^ hay(u(t  -  rf))  +  ^cr3y(v(t  -  y))3  =  0, 

(38) 

where  y"(-)  denotes  the  second  derivative  of  y(-)  with 
respect  to  its  argument.  Defining  m  such  that 

m  =  £2  and  1~2m=  -^2>  <39> 

we  obtain 

y"  +  (1  —  2  m)y  +  2  my3  =  0,  (40) 

which  has  as  its  solution  the  Jacobi  elliptic  function 

y  =  cn(v(t- y),m),  (41) 

provided  0  <  m  <  1.  (Note  that  this  m  is  unrelated  to 

the  earlier  definition  of  m  as  the  number  of  control  inputs 
per  particle  in  G.  Henceforth,  we  use  m  to  refer  only  to 
the  parameter  of  the  Jacobi  elliptic  function.)  Thus,  we  may 
conclude  that  solutions  to  (36)  are  of  the  form 

Ft  (t)  =  (2V2  vyfrnj  m=^(l+  ^  ^ , 

V2) 

with  v 2  >  \h\/2.  We  then  have  the  optimal  control  u  given 
by  u  =  y,\. 

Remark:  For  G  =  SE{2),  we  can  express  the  dynamics  as 
(32)  with  fixed  endpoints 

m  =e0,  r(0)  =  ro,  6(T)=0t,  r(T)  =  rT.  (43) 


B.  Single-particle  optimization  problem 

For  a  single  particle  in  SE( 2),  consider  minimization  of 

C=\JQ  l^“WI  2dt=\JQ  {l  +  u(t)2)dt,  (33) 

subject  to  the  dynamics  g  =  g^u,  t;u  =  X2  +  Xi u,  and  the 
endpoint  conditions  g(0)  =  go,  g(T)  =  gx-  For  this  single 
particle  fixed  endpoint  problem  we  have  the  Lie-Poisson 
reduced  dynamics 


d 

dt 


Ft 
F2 
F  3 


1 

V2 


0 

-F3 

F2 


F  3 
0 
0 


-F  2 
0 
0 


Fi 

1 

0 

,  (34) 


along  with  the  conserved  quantities 


h  —  F2  +  ^Fi)  c  —  2  (f 2  +  F3)  1 


(35) 


the  (reduced)  hamiltonian  and  Casimir  function,  respectively 
[14],  (Here  pi,  p-2,  and  are  the  scalar  components  of  the 
single  vector  p  £  se(2)*.)  We  thus  have 


Ft  =  ^F3  =  ^F2Fi  =  \ 


Pi- 


(36) 


Thus,  the  endpoint  conditions  involve  not  only  the  position 
endpoints  in  the  plane,  ro  and  r-p,  but  also  the  tangent  vectors 
to  the  trajectory  at  the  endpoints,  90  and  9p.  Note  that  (42) 
has  three  parameters,  v,  rj,  and  to,  as  degrees  of  freedom 
for  meeting  the  endpoint  conditions  (which  also  represent 
three  degrees  of  freedom,  because  without  loss  of  generality 
we  can  take  ro  =  (0, 0)  and  9o  =  0).  It  is  clear  that  a 
solution  satisfying  a  given  set  of  endpoint  conditions  (43) 
exists  provided  |r^  —  ro|  <  T.  It  is  thus  reasonable  to  speak 
of  optimal  solutions,  provided  —  ro|  <  T. 

C.  Special  class  of  coupled  solutions 

For  the  coupled  system  of  N  >  1  particles  in  SE( 2), 
we  have  not  found  a  closed-form  solution  like  (42)  that 
applies  in  general.  However,  there  are  special  solutions  to 
(22)  with  G  =  SE(2)  which  do  take  the  form  of  Jacobi 
elliptic  functions. 

Suppose  y  and  B  are  fixed,  and  consider  the  following 
class  of  candidate  solutions,  analogous  to  (42): 

Ffci  =  OfcCn(z/(f  -  ??),  to),  k=l,...,N,  (44) 
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where  Ok  >  0,  k  =  1, N,  v,  r],  and  to  are  given  constants. 
Then 


dh/dpn 

cri 

dh/dp  21 

cr2 

=  T 

dh/dpNi 

on 

cn  (i/(t-r)),m),  (45) 


and  from  (26)  we  have 


h  =  ^2  Mfc2  +  -  [cri  -  •  <7n ] 

fc= l 

(46) 

We  also  have  N  Casimirs  of  the  form  Ck  =  \ ( /r/2  +  /x^3), 

/,  =  1 . V. 

We  would  like  to  find  all  possible  choices  of  <Ti, ct/v,  i', 
77,  and  to  for  which  (44)  solves  system  (22)  with  G  =  SE(2). 
However,  we  leave  this  problem  for  future  work,  and  instead 
here  ask  the  simpler  question:  given  fixed  positive  values  for 
cri, (Tjv,  how  do  they  constrain  the  remaining  constants, 
and  what  form  must  the  controls  u\,  ...,Un  take? 

Suppose  that  Pi,  P2,  ■■■■> Pn,  positive  constants,  satisfy 


<?i 


on 


cn  {v{t  —  77),  ?ro)2 


[err  •  •  •  <7jv]  diag(/?!, ...,  pN) 

cri 

=  [<7l  '  ■■On}'& 

CTl 

on 

on 

(47) 


(Note  that  for  cri,...,  aw  given,  this  amounts  to  one  scalar 
equation  relating  the  N  variables  Pi,...,Pn.)  Then  we  can 
define  ^ 

hk  =  Hk2  +  A-=  L.....Y.  (48) 

and  it  follows  that  h  =  Y^,k= 1  Y-  Furthermore, 


dh/dpu 

CTl 

=  diag(/3i, ...,  pN) 

dh/dpNi 

_on  _ 

cn  (u(t  —  r]),m), 
(49) 


and  we  obtain  the  reduced  dynamics 


Afci 

1 

0  Pk3  ~Pk2 

PkPkl 

Pk2 

— 

~Pk3  0  0 

1 

Pk3 

v  2 

Pk2  0  0 

0 

rso) 


for  k  =  1, N,  which  leads  to 

Mfci  =  y  “  ^fc/41)  Mfcl)  fc=l,...,7V.  (51) 

But  (51)  is  satisfied  by  (44)  when 

P?o?  Pkhk 

hk  =  constant,  m  =  — 1  -  2m  = — — (52) 
4z/^  2vz 

for  all  k  =  1,  ...,N.  Note  that  (52)  implies 


PjCTj  =  PkVk,  j,k=l,...,N,  (53) 

which  imposes  N  —  1  independent  constraints  on  /3i, ...,  Pn- 
Along  with  (47),  there  are  a  total  of  N  independent  scalar 
equations  for  the  N  quantities  p-\ , Pn  we  seek  to  solve 


for  (given  cj\, ...,  <jn).  If  a  solution  / 3\,..,Pm  >  0  to  these 
equations  exists,  we  say  that  the  collection  ctjv  is 

admissible. 

To  summarize,  the  reduced  dynamics  for  the  coupled 
system  of  N  particles  (with  \  ancl  B  fixed  a  priori)  is 
given  by  (22).  However,  we  focus  on  special  solutions  to 
(22),  having  the  form  (44),  where  the  collection  of  positive 
constants  is  admissible.  Then  v ,  77,  and  m  exist 

(indeed,  77  is  arbitrary,  and  we  can  choose  v  so  that  0  < 
to  <  1),  such  that  (44)  is  in  fact  a  solution  to  (22).  Finally, 
because  the  optimal  controls  can  be  related  to  the  reduced 
variables  through 


we  have 


u  1 


UN 


Ui 

=  T 

P11 

5 

UN 

P  N1 

=  T 

cri 

cn  (v(t 

-  77),  to) 

on 

D.  Interpretation  of  special  solutions 


(54) 


(55) 


The  special  solutions  (55)  have  the  property  that  each 
particle’s  steering  control  is  proportional  to  every  other 
particle’s  steering  control.  We  only  expect  such  special 
solutions  to  apply  to  a  thin  set  of  endpoint  conditions  for  a 
system  of  N  particles.  However,  the  definition  of  this  space 
of  special  solutions  is  not  vacuous:  there  are  examples  of 
sets  of  endpoint  conditions  consistent  with  coupled  optimal 
solutions  having  the  form  (55). 

In  fact,  despite  applying  to  only  a  thin  set  of  endpoint 
conditions,  there  is  a  reasonable  amount  of  freedom  in  (55) 
through  the  choice  of  a\, on-  We  can  generate  collections 
of  trajectories  using  (55),  and  then  state  that  among  all 
possible  collections  of  curves  satisfying  the  same  endpoint 
conditions,  the  trajectories  we  have  generated  satisfy  the 
(necessary)  condition  for  optimality  of  the  coupled  system. 


E.  Numerical  example 

Suppose  we  take  N  =  2,  %  =  1, 

1  -1  ' 
-1  1 

o  1  =  1,  and  (7 2  =  1/2.  Then  we  have 


B  = 


*  =  (In  +  2\B)~  = 


(56) 


1  _ 

1  +  2% 

-2X  ' 

1 

'3  2 

.  ~2X 

l  +  2x. 

~  5 

2  3 

L  /4T 

From 


Pl<?i  +  P2crl  =  [  cr\  cr2  ]  T 


o-i 
(72 
3  2 
2  3 


23 

=  40’(58) 


and  Pi<ti  =  P2V2  =>  Pi  =  \P2~  we  obtain  Pi  =  23/60, 
P2  =  23/30.  Choosing  T  =  6,  m  =  and  77  =  0,  we  have 


Picri 

2m 


23 

60’ 


(59) 
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Fig.  1.  Controls  u\(t)  and  U2(t)  given  by  (60)  plotted  vs.  time  t. 


Fig.  2.  Planar  trajectories  corresponding  to  the  steering  controls  in  figure  1. 
The  short  bold  arrows  indicate  the  corresponding  fixed  endpoint  conditions. 

so  that,  finally, 

1  4 

u\{t)  =  -  (3/xn  +  2^21)  =  -cn(v(t,  -  ??), to), 

1  7 

u2{t)  =  ^  (2/in  +  3^21)  =  T^cn (v(t  -  77),  to),  (60) 
5  10 

for  t  £  [0,T],  Figure  1  shows  U\(t)  and  712(f)  given  by  (60). 
Figure  2  shows  the  corresponding  planar  trajectories. 

Two  caveats  about  this  example  merit  emphasis.  First,  we 
have  chosen  initial  headings  which  are  aligned:  other  than 
through  boundary  conditions,  the  steering  controls  do  not 
feel  the  effects  of  relative  positions  and  headings  between 
the  particles,  and  so  there  is  no  guarantee  of  noncollision  in 
this  setting.  Second,  the  endpoint  conditions  in  this  example 
are  such  that  the  special  solutions  (based  on  proportional 
elliptic  function  steering  controls)  apply;  for  arbitrary  end¬ 
point  conditions,  these  special  solutions  need  not  apply,  and 
we  must  resort  to  solving  (22),  e.g.,  numerically. 

IV.  Directions  for  future  work 

For  planar  trajectories,  the  correspondence  between  a  par¬ 
ticle  in  R2  moving  along  a  (smooth)  trajectory,  and  a  particle 
(i.e.,  a  group  element)  evolving  in  SE( 2)  is  unambiguous. 


Associated  with  the  particle  trajectory  in  K2,  there  is  an 
orthonormal  frame  consisting  of  a  unit  tangent  vector  and 
unit  normal  vector,  and  these  vectors  along  with  the  position 
vector  map  in  a  one-to-one  fashion  to  elements  of  SE( 2). 
A  particle  in  R3'  moving  along  a  (smooth)  trajectory  has  an 
intrinsically  defined  unit  tangent  vector  and  corresponding 
normal  plane,  but  this  is  not  enough  to  have  a  one-to-one 
mapping  to  elements  of  SE( 3).  This  introduces  subtleties  in 
interpreting  results  for  moving  particles  in  S h'(?>)  in  terms 
of  curves  and  frames  in  R3.  Single-particle  fixed  endpoint 
optimal  control  problems  on  SE(3)  (with  generalizations  to 
SE(n))  are  explored  in  [10]. 

In  his  recent  work  [1]  Brockett  investigates  the  collective 
and  individual  behaviors  of  a  set  of  identical  agents  with 
nonlinear  dynamics,  all  subject  to  identical  controls,  and 
asks  questions  about  collective  response  to  a  coordination 
signal  broadcast  by  a  leader.  In  such  a  setting  he  shows 
that  nonlinearity  plays  a  critical  role.  While  his  focus  is  on 
closed  loop  effects  (whereas  our  subject  is  open  loop  optimal 
control),  it  may  be  useful  to  examine  the  results  of  section 
III.C  on  special  proportional  controls  from  the  perspective 
of  his  paper. 
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